#include "MG4005_driver.h"
#include "BSP.h"
#include <stdarg.h>

static volatile uint8_t IsReceived = 0;
static volatile uint8_t *pData;

void MG_CAN_Callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    switch (pHeader->StdId)
    {
    case MG_M1_ID:
    case MG_M2_ID:
    case MG_M3_ID:
    case MG_M4_ID:
    case MG_M5_ID:
    {
        IsReceived = 1;
        pData = pBuf;
    }
    }
}

void MG_Init(MG_measure_t *MG, int MG_Motor_Count, ...)
{
    // MG_Count = (uint8_t)MG_Motor_Count;

    va_list arg;
    va_start(arg, MG_Motor_Count);

    for (uint8_t i = 0; i < MG_Motor_Count; i++)
    {
        MG[i].ID = va_arg(arg, int);
    }

    va_end(arg);
}

void MG_WriteZero(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x19;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);
}

void MG_ReadAngle(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x92;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->angle = (int32_t)(pData[4] << 24 | pData[3] << 16 | pData[2] << 8 | pData[1]) * 0.01;
}

void MG_ReadOneAngle(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x94;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->angle = (uint32_t)(pData[7] << 24 | pData[6] << 16 | pData[5] << 8 | pData[4]) * 0.01;
}

void MG_ReadErrorState(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x9A;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->temperature = pData[1];
}

void MG_Close(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x80;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);
}

void MG_Stop(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x81;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);
}

void MG_Run(MG_measure_t *MG)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x88;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);
}

void MG_SetSpeed(MG_measure_t *MG, float speed)
{
    int32_t Speed = (int32_t)(speed * 1000);
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA2;
    cmd[4] = Speed;
    cmd[5] = Speed >> 8;
    cmd[6] = Speed >> 16;
    cmd[7] = Speed >> 24;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->temperature = pData[1];
    MG->current = (int16_t)(pData[3] << 8 | pData[2]) / 2048 * 33;
    MG->speed = (int16_t)(pData[5] << 8 | pData[4]) / 10;
    MG->encoder = (uint16_t)(pData[7] << 8 | pData[6]);
}

void MG_SetAngle(MG_measure_t *MG, float angle, uint16_t maxSpeed)
{
    int32_t Angle = (int32_t)(angle * 1000);
    uint16_t Speed = maxSpeed * 10;
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA4;
    cmd[4] = Speed;
    cmd[5] = Speed >> 8;
    cmd[4] = Angle;
    cmd[5] = Angle >> 8;
    cmd[6] = Angle >> 16;
    cmd[7] = Angle >> 24;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->temperature = pData[1];
    MG->current = (int16_t)(pData[3] << 8 | pData[2]) / 2048 * 33;
    MG->speed = (int16_t)(pData[5] << 8 | pData[4]) / 10;
    MG->encoder = (uint16_t)(pData[7] << 8 | pData[6]);
}

void MG_SetOneAngle(MG_measure_t *MG, float angle, uint8_t dir, uint16_t maxSpeed)
{
    uint16_t Angle = (uint16_t)(angle * 1000);
    uint16_t Speed = maxSpeed * 10;
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA6;
    cmd[1] = dir;
    cmd[2] = Speed;
    cmd[3] = Speed >> 8;
    cmd[4] = Angle;
    cmd[5] = Angle >> 8;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->temperature = pData[1];
    MG->current = (int16_t)(pData[3] << 8 | pData[2]) / 2048 * 33;
    MG->speed = (int16_t)(pData[5] << 8 | pData[4]) / 10;
    MG->encoder = (uint16_t)(pData[7] << 8 | pData[6]);
}

void MG_SetAngleInc(MG_measure_t *MG, float angle, uint16_t maxSpeed)
{
    int32_t Angle = (uint16_t)(angle * 1000);
    uint16_t Speed = maxSpeed * 10;
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA8;
    cmd[2] = Speed;
    cmd[3] = Speed >> 8;
    cmd[4] = Angle;
    cmd[5] = Angle >> 8;
    cmd[6] = Angle >> 16;
    cmd[7] = Angle >> 24;
    CAN_Transmit(&MG_CAN, MG->ID, cmd);

    while (!IsReceived)
        continue;
    IsReceived = 0;

    MG->temperature = pData[1];
    MG->current = (int16_t)(pData[3] << 8 | pData[2]) / 2048 * 33;
    MG->speed = (int16_t)(pData[5] << 8 | pData[4]) / 10;
    MG->encoder = (uint16_t)(pData[7] << 8 | pData[6]);
}
